#include "track/object_track.h"
#include "opencv2/opencv.hpp"

namespace autodrive {
namespace perception {
int32_t s_track_cached_history_size_maximum = 5;

IdGenerator ObjectTrack::id_generator_{};

// ObjectTrack::ObjectTrack() { idx_ = id_generator_.GetId(); }

ObjectTrack::ObjectTrack(TrackedObjectPtr obj) {
  idx_ = id_generator_.GetId();
  // std::cout << "ObjectTrack constrctor id = " << idx_ << std::endl;
  current_object_ = obj;
}

ObjectTrack::~ObjectTrack() { id_generator_.ReleaseId(idx_); }

void ObjectTrack::UpdateWithObject(TrackedObjectPtr* new_object,
                                   double time_diff,
                                   const MapTrackParameter& setting) {
  // A. update object track
  // center point
  // std::cout << "before position " << (*new_object)->center.x() << " "
  //           << (*new_object)->center.y() << " " << (*new_object)->center.z()
  //           << std::endl;
  position_filter_.AddMeasurement((*new_object)->center,
                                  setting.position_filter_alpha);
  (*new_object)->center = position_filter_.GetState();
  // std::cout << "after position " << (*new_object)->center.x() << " "
  //           << (*new_object)->center.y() << " " << (*new_object)->center.z()
  //           << std::endl;
  // length
  // std::cout << "before length " << (*new_object)->length << std::endl;
  length_filter_.AddMeasurement((*new_object)->length,
                                setting.length_filter_alpha);
  (*new_object)->length = length_filter_.GetState();
  // std::cout << "after length " << (*new_object)->length << std::endl;
  // width
  // std::cout << "before width " << (*new_object)->width << std::endl;
  width_filter_.AddMeasurement((*new_object)->width,
                               setting.width_filter_alpha);
  (*new_object)->width = width_filter_.GetState();
  // std::cout << "after width " << (*new_object)->width << std::endl;
  // yaw
  // std::cout << "before yaw " << (*new_object)->yaw << std::endl;
  yaw_filter_.AddMeasurement((*new_object)->yaw, setting.yaw_filter_alpha);
  (*new_object)->yaw = yaw_filter_.GetState();
  // std::cout << "after yaw " << (*new_object)->yaw << std::endl;

  // // get polygen point
  // cv::RotatedRect rect{
  //     cv::Point2f{(*new_object)->center.x(), (*new_object)->center.y()},
  //     cv::Size2f{(*new_object)->width, (*new_object)->length},
  //     (*new_object)->yaw + 90};
  // cv::Mat mat;
  // cv::boxPoints(rect, mat);
  // int num = (*new_object)->polygon.GetNumberFilled();
  // (*new_object)->polygon.Clear();
  // for (int i = 0; i < num; ++i) {
  //   Eigen::Vector2f& vec = (*new_object)->polygon.GetNext();
  //   vec.x() = mat.at<float>(i, 0);
  //   vec.y() = mat.at<float>(i, 1);
  //   (*new_object)->polygon.Add();
  // }

  // A.2 update track info
  ++age_;
  total_visible_count_++;
  consecutive_invisible_count_ = 0;
  // A.3 update history
  // A.4 update history
  std::int32_t history_size = history_objects_.size();
  if (history_size >= s_track_cached_history_size_maximum) {
    history_objects_.pop_front();
  }
  history_objects_.push_back(current_object_);
  current_object_ = *new_object;
  // B. smooth object track
  // B.1 smooth velocity
  // smooth_track_velocity((*new_object), time_diff);
  // B.2 smooth orientation
  //  SmoothTrackOrientation();
}

void ObjectTrack::UpdateWithoutObject(Eigen::VectorXf const& predict_state,
                                      double time_diff) {
  // A. Update object of track
  TrackedObjectPtr new_obj(new TrackedObject());

  new_obj->id = current_object_->id;
  new_obj->center =
      Eigen::Vector3f{current_object_->center.x(), current_object_->center.y(),
                      current_object_->center.z()};
  new_obj->length = current_object_->length;
  new_obj->width = current_object_->width;
  new_obj->height = current_object_->height;
  new_obj->yaw = current_object_->yaw;
  new_obj->score = current_object_->score;
  new_obj->association_score = current_object_->association_score;
  new_obj->polygon = current_object_->polygon;
  new_obj->points = current_object_->points;

  // center point
  position_filter_.AddMeasurement(new_obj->center, 0.5f);
  new_obj->center = position_filter_.GetState();
  // length
  length_filter_.AddMeasurement(new_obj->length, 0.5f);
  new_obj->length = length_filter_.GetState();
  // width
  width_filter_.AddMeasurement(new_obj->width, 0.5f);
  new_obj->width = width_filter_.GetState();
  // yaw
  width_filter_.AddMeasurement(new_obj->yaw, 0.5f);
  new_obj->yaw = width_filter_.GetState();

  // B. update track info
  ++age_;
  consecutive_invisible_count_++;

  // C. update history
  std::int32_t history_size = history_objects_.size();
  if (history_size >= s_track_cached_history_size_maximum) {
    history_objects_.pop_front();
  }
  history_objects_.push_back(current_object_);
  current_object_ = new_obj;
}

////////////object track set//////////
float s_track_visible_ratio_minimum_ = 0.6;
std::int32_t s_track_consecutive_invisible_maximum_ = 4;

ObjectTrackSet::ObjectTrackSet() {
  tracks_.reserve(300U);
  ObjectTrack::InitIdGenerator();
}
ObjectTrackSet::~ObjectTrackSet() {}

std::int32_t ObjectTrackSet::RemoveLostTracks() {
  std::int32_t track_num = 0;
  for (std::uint16_t i = 0; i < tracks_.size(); ++i) {
    // A. remove tracks invisible ratio is less than given minimum
    if (tracks_[i]->age_ == 0) {
      continue;
    } else {
      float track_visible_ratio =
          tracks_[i]->total_visible_count_ * 1.0f / tracks_[i]->age_;
      if (track_visible_ratio < s_track_visible_ratio_minimum_) {
        continue;
      }
    }
    // B. remove tracks consecutive invisible count greater than given maximum
    std::int32_t track_consecutive_invisible_count =
        tracks_[i]->consecutive_invisible_count_;
    if (track_consecutive_invisible_count >
        s_track_consecutive_invisible_maximum_) {
      continue;
    }
    // C. change order, move those objects who needs to be removed to the end of
    // the vector
    if (i == track_num) {
      track_num++;
    } else {
      ObjectTrackPtr tmp{tracks_[i]};
      tracks_[i] = tracks_[track_num];
      tracks_[track_num] = tmp;
      track_num++;
    }
  }

  // Remove lost tracks
  std::int32_t no_removed = tracks_.size() - track_num;
  for (size_t i = track_num; i < tracks_.size(); ++i) {
    if (tracks_[i] != nullptr) {
      delete (tracks_[i]);
      tracks_[i] = nullptr;
    }
  }
  tracks_.resize(track_num);
  return no_removed;
}

}  // namespace perception
}  // namespace autodrive